﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;

namespace TreeLayoutPlayground
{
    class SpringLayoutEngine2 : LayoutEngine
    {
        public override void Layout(Tree tree)
        {
            SpringSystem system = new SpringSystem(tree);
            long steps = system.Run(0.001);
            Console.WriteLine("Steps: {0}", steps);
        }

        private class SpringSystem
        {
            public struct Vector
            {
                public static readonly Vector XAxis = new Vector { X = 1, Y = 0 };
                public static readonly Vector YAxis = new Vector { X = 0, Y = 1 };

                public double X { get; set; }
                public double Y { get; set; }

                public double Length { get { return Math.Sqrt((X * X) + (Y * Y)); } }

                public void Normalize()
                {
                    double l = Length;
                    X /= l;
                    Y /= l;
                }

                public double Distance(Vector o)
                {
                    double dx = X - o.X;
                    double dy = Y - o.Y;

                    return Math.Sqrt((dx * dx) + (dy * dy));
                }

                public static Vector operator -(Vector a)
                {
                    return new Vector
                    {
                        X = -a.X,
                        Y = -a.Y
                    };
                }

                public static Vector operator +(Vector a, Vector b)
                {
                    return new Vector
                    {
                        X = a.X + b.X,
                        Y = a.Y + b.Y
                    };
                }

                public static Vector operator -(Vector a, Vector b)
                {
                    return new Vector
                    {
                        X = a.X - b.X,
                        Y = a.Y - b.Y
                    };
                }

                public static Vector operator *(Vector a, double b)
                {
                    return new Vector
                    {
                        X = a.X * b,
                        Y = a.Y * b
                    };
                }

                public static Vector operator *(double b, Vector a)
                {
                    return new Vector
                    {
                        X = a.X * b,
                        Y = a.Y * b
                    };
                }

                public static Vector operator /(Vector a, double b)
                {
                    return new Vector
                    {
                        X = a.X / b,
                        Y = a.Y / b
                    };
                }
            }

            private class Node
            {
                public static int GapSize = 16;

                public Tree Tree;
                public Vector Position;
                public Vector Velocity;
                public Vector Acceleration;
                public Size Size;
                public double Mass;
                public bool Mobile;

                public Node(Tree t, double ma, bool mo = true)
                {
                    Tree = t;
                    Position = new Vector
                    {
                        X = t.MyBox.Width / 2,
                        Y = t.MyBox.Height / 2
                    };
                    Velocity = new Vector();
                    Acceleration = new Vector();
                    Size = new Size(
                        t.MyBox.Width + GapSize,
                        t.MyBox.Height + GapSize);
                    Mass = ma;
                    Mobile = mo;
                }

                public void AddForce(Vector f)
                {
                    Acceleration += f / Mass;
                }

                public void Update(double dt, Vector gravity, double drag)
                {
                    if (Mobile)
                    {
                        Velocity += dt * (Acceleration + gravity);
                        Position += dt * Velocity;
                        Velocity *= (1 - drag);
                    }

                    Acceleration.X = Acceleration.Y = 0;
                }

                public void Repel(Node o)
                {
                    Vector dir = Position - o.Position;
                    double distance = dir.Length;
                    dir /= distance;

                    if (distance > (Math.Max(Size.Width, Size.Height) / 2)) { return; }

                    double force = -100000 / (distance * distance);
                    o.AddForce(new Vector
                    {
                        X = dir.X * force,
                        Y = dir.Y * force * Size.Width / Size.Height
                    });
                }
            }

            private class Spring
            {
                public Node Left;
                public Node Right;
                public double Constant;
                public double RestLength;

                public Spring(Node l, Node r, double c, double rl)
                {
                    Left = l;
                    Right = r;
                    Constant = c;
                    RestLength = rl;
                }

                public void ApplyForce()
                {
                    Vector dir = Left.Position - Right.Position;
                    double len = dir.Length;
                    dir /= len;

                    double force = -Constant * (len - RestLength);

                    Left.AddForce(force * dir);
                    Right.AddForce(-force * dir);
                }
            }

            public static int GapSize = 16;
            public static double TimeStep = 0.01;
            public static Vector Gravity = new Vector { X = 0, Y = 9.81 };
            public static double Drag = 0.01;

            private List<Node> nodes;
            private List<Spring> springs;
            private Tree root;

            public SpringSystem(Tree tree)
            {
                Node.GapSize = GapSize;

                root = tree;
                nodes = new List<Node>();
                springs = new List<Spring>();

                Queue<Node> work = new Queue<Node>();
                Node top = new Node(tree, 100, false);
                nodes.Add(top);
                work.Enqueue(top);

                do
                {
                    Node cur = work.Dequeue();
                    double shift = 0;
                    foreach (Tree c in cur.Tree.Children)
                    {
                        Node child = new Node(c, 100);
                        child.Position += cur.Position + new Vector { X = shift, Y = cur.Size.Height };
                        shift += child.Size.Width;

                        springs.Add(new Spring(cur, child, 500, 4 * GapSize));

                        nodes.Add(child);
                        work.Enqueue(child);
                    }
                } while (work.Count > 0);
            }

            public double Step()
            {
                foreach (Spring s in springs) { s.ApplyForce(); }

                foreach (Node a in nodes)
                {
                    foreach (Node b in nodes)
                    {
                        if (a == b) { continue; }

                        a.Repel(b);
                    }
                }

                double energy = 0;
                foreach (Node a in nodes)
                {
                    a.Update(TimeStep, Gravity, Drag);

                    double speed = a.Velocity.Length;
                    energy += 0.5 * a.Mass * speed * speed;
                }

                return energy;
            }

            public long Run(double maxEnergy)
            {
                // Run simulation
                long steps = 0;
                double curEnergy;
                do
                {
                    steps++;
                    curEnergy = Step();
                    Console.WriteLine("Energy: {0}", curEnergy);
                } while (curEnergy > maxEnergy);

                // Update positions
                Rectangle bound = new Rectangle(0, 0, 0, 0);
                foreach (Node a in nodes)
                {
                    a.Tree.MyBox.X = (int)a.Position.X - (a.Tree.MyBox.Width / 2);
                    a.Tree.MyBox.Y = (int)a.Position.Y - (a.Tree.MyBox.Height / 2);
                    a.Tree.MyRelativeTranslation = Size.Empty;

                    bound = Rectangle.Union(bound, a.Tree.MyBox);
                }

                foreach (Node a in nodes)
                {
                    a.Tree.MyBox.Offset(-bound.X, -bound.Y);
                }

                root.OverallSize = new Size(
                    1 + bound.Width,
                    1 + bound.Height);

                return steps;
            }
        }
    }
}
